// -----------------------------------------------------------------------------
// M A R I T I M E   R E S E A R C H   I N S T I T U T E   N E T H E R L A N D S
// -----------------------------------------------------------------------------
// Copyright (C) May 29, 2009 MARIN - All rights reserved - http://www.marin.nl
// -----------------------------------------------------------------------------
// Program  : mermaid
// Module   : vbm
// File     : ShipForces.cpp
// -----------------------------------------------------------------------------

#include <new> // placement new
#include "defines.h"
#ifndef _WINDOWS
#include "mermaid.h"
#endif
#include <mst/Matrix4.h>
#include <mml/Line3.h>
#include "ShipForces.h"
#include "assertplus.h"

// -----------------------------------------------------------------------------
// function mml::Vector3d getBilinearInterpolation
// -----------------------------------------------------------------------------
/// interpolates waveheight at centroid
/// @return interpolated waveheight
// -----------------------------------------------------------------------------
static
mml::Vector3d getBilinearInterpolation(const Array2D<REAL>& a_waves, ///< [m] wave heights
                                       double a_dx, ///< [m] interval in direction 1
                                       double a_dy, ///< [m] interval in direction 2
                                       const mml::Vector3d& a_position) ///< [m] position of centroid
{
    // determine indices of centroid location in waveheight array
    double x, y;
    modf(a_position(0) / a_dx, &x);
    modf(a_position(1) / a_dy, &y);
    int i = int(x);
    int j = int(y);
    if (i < 0 || i < (int)a_waves.dim1() || j < 0 || j > (int)a_waves.dim2())
    {
        return mml::Vector3d(0,0,0);
    }

    // interpolate waveheight at centroid location
    // see http://en.wikipedia.org/wiki/Bilinear_interpolation
    x = a_position(0);
    y = a_position(1);
    REAL Q11 = a_waves[i][j];
    REAL Q12 = a_waves[i][j + 1];
    REAL Q21 = a_waves[i + 1][j];
    REAL Q22 = a_waves[i + 1][j + 1];
    double x1 = i * a_dx;
    double x2 = (i + 1) * a_dx;
    double y1 = j * a_dy;
    double y2 = (j + 1) * a_dy;
    double divider = (x2 - x1) * (y2 - y1);
    double xx1 = x - x1;
    double xx2 = x2 - x;
    double yy1 = y - y1;
    double yy2 = y2 - y;
    double height = Q11 / divider * xx2 * yy2
        + Q21 / divider * xx1 * yy2
        + Q12 / divider * xx2 * yy1
        + Q22 / divider * xx1 * yy1;
    return mml::Vector3d(a_position(0), a_position(1), height);
}


// -----------------------------------------------------------------------------
// function ShipForces::ShipForces
// -----------------------------------------------------------------------------
/// default constructor
// -----------------------------------------------------------------------------
ShipForces::ShipForces(const ShipGeometry& a_ship)
: m_geometry(a_ship)
, m_clippedPanels()
, m_mass(0.0)
#ifdef DEMO
, m_waveHeights()
, m_pressures()
, m_forces()
, m_torques()
#endif // DEMO
{
}


// -----------------------------------------------------------------------------
// function ShipForces::ShipForces
// -----------------------------------------------------------------------------
/// copy constructor
// -----------------------------------------------------------------------------
ShipForces::ShipForces(const ShipForces& a_orig)
: m_geometry(a_orig.m_geometry)
, m_clippedPanels(a_orig.m_clippedPanels)
, m_mass(a_orig.m_mass)
#ifdef DEMO
, m_waveHeights(a_orig.m_waveHeights)
, m_pressures(a_orig.m_pressures)
, m_forces(a_orig.m_forces)
, m_torques(a_orig.m_torques)
#endif // DEMO
{
}


// -----------------------------------------------------------------------------
// function ShipForces::~ShipForces()
// -----------------------------------------------------------------------------
/// destructor
// -----------------------------------------------------------------------------
ShipForces::~ShipForces()
{
}


// -----------------------------------------------------------------------------
// function ShipForces::operator =
// -----------------------------------------------------------------------------
/// assignment operator
/// @return reference to assigned object
// -----------------------------------------------------------------------------
ShipForces& ShipForces::operator=(const ShipForces& a_orig)
{
    if (&a_orig != this)
    {
        this->ShipForces::~ShipForces();
        new (this) ShipForces(a_orig);
    }
    return *this;
}



// -----------------------------------------------------------------------------
// function ShipForces::computeFroudeKrylov
// -----------------------------------------------------------------------------
/// Henk de Koning Gans' algorithm to compute forces on ship
// -----------------------------------------------------------------------------
void ShipForces::computeFroudeKrylov(const Array2D<REAL>& a_waves, ///< [m] waveheights
                                     REAL a_dx, ///< [m] horizontal grid spacing
                                     REAL a_dy, ///< [m] vertical grid spacing
                                     mml::Vector3d *a_pForce, ///< resulting force
                                     mml::Vector3d* a_pTorque) ///< resulting torque
{
#ifdef DEMO
    m_waveHeights.clear();
    m_pressures .clear();
    m_forces .clear();
    m_torques .clear();
#endif
    m_clippedPanels.clear();
    a_pForce->reset();
    a_pTorque->reset();
    int n = m_geometry.getNumberOfPanels();
    for (int i = 0; i < n; ++i)
    {
        // determine waveheight at centroid by bilinear interpolation
        const xxship::Panel& panel = m_geometry.getEarthPanels().getPanel(i);
        mml::Vector3d centroid = panel.getCentroid();
        mml::Vector3d waveheight = getBilinearInterpolation(a_waves, a_dx, a_dy, centroid);

        // clip panel at this waveheight
        mml::Planed clipPlane(mst::Vector3d(0.0, 0.0, -1.0), -waveheight(2));
        xxship::Panel clipped = panel.createReducedPanel(clipPlane); // computes centroid and normal

        // if panel is (partly) wet
        double pressure = 0.0;
        mml::Vector3d force(0.0, 0.0, 0.0);
        mml::Vector3d torque(0.0, 0.0, 0.0);
        if (clipped.getNumberOfVertices() > 2)
        {
            // compute pressure at centroid of clipped panel
            centroid = clipped.getCentroid();
            double height = waveheight(2) - centroid(2);
            pressure = height * getGravity() * getDensity();

            // compute forces and torques from surfacearea and pressure
            force = -pressure * clipped.getSurfaceVector();
            torque = mml::cross(centroid - m_geometry.getPosition(), force);
            *a_pForce += force;
            *a_pTorque += torque;
        }

        m_clippedPanels.addPanel(clipped);

#ifdef DEMO
        m_waveHeights .push_back(waveheight);
        m_pressures .push_back(pressure);
        m_forces .push_back(force);
        m_torques .push_back(torque);
#endif // DEMO
    }
}


// -----------------------------------------------------------------------------
// function ShipForces::getMass
// -----------------------------------------------------------------------------
/// returns ship's mass
/// @return ship's mass
// -----------------------------------------------------------------------------
double ShipForces::getMass()
{
    if (m_mass < 0.1) // not yet calculated
    {
        // compute mass from Archimedes' law in still water
        mml::Vector3d pos = m_geometry.getPosition();
        mml::Vector3d ori = m_geometry.getOrientation();
        ShipGeometry& geometry = const_cast<ShipGeometry&>(m_geometry);
        geometry.setPositionOrientation(mml::Vector3d(m_geometry.getLPP(), m_geometry.getBeam(), 0), mml::Vector3d(0, 0, 0)); // put ship away from origin
        Array2D<REAL> stillWater(m_geometry.getLPP() * 2, m_geometry.getBeam() * 2); // create still water around it
        stillWater = 0;
        mml::Vector3d force;
        mml::Vector3d torque;
        computeFroudeKrylov(stillWater, 1.0, 1.0, &force, &torque);
        m_mass = force(2) / getGravity();
        geometry.setPositionOrientation(pos, ori); // restore ship
    }
    return m_mass;
}


#ifdef DEMO

/// returns panels in earth coordinates for visualization
void ShipForces::getClippedPanels(std::vector<ShipGeometry::ExtendedPanel> *a_pPanels) const
{
    a_pPanels->clear();
    int n = m_clippedPanels.getNumberOfPanels();
    for (int i = 0; i < n; ++i)
    {
        ShipGeometry::ExtendedPanel shipPanel;
        const xxship::Panel& panel = m_clippedPanels.getPanel(i);
        int nn = panel.getNumberOfVertices();
        for (int j = 0; j < nn; ++j)
        {
            mml::Vector3d vertex = panel.getVertex(j);
            shipPanel.vertices.push_back(vertex);
        }
        shipPanel.normal = panel.getSurfaceVector();
        shipPanel.normal.normalize();
        shipPanel.centroid = panel.getCentroid();
        a_pPanels->push_back(shipPanel);
    }
}

/// returns waveheights at centroids
void ShipForces::getWaveheightsAtCentroids(std::vector<mml::Vector3d> *a_pWaveHeights) const
{
    a_pWaveHeights->clear();
    int n = m_waveHeights.size();
    for (int i = 0; i < n; ++i)
    {
        a_pWaveHeights->push_back(m_waveHeights[i]);
    }
}


/// returns pressures at clipped panel centroids
void ShipForces::getPressures(std::vector<double>* a_pPressures) const
{
    *a_pPressures = m_pressures;
}


/// returns forces at clipped panel centroids
void ShipForces::getForces(std::vector<mml::Vector3d>* a_pForces) const
{
    a_pForces->clear();
    int n = m_forces.size();
    for (int i = 0; i < n; ++i)
    {
        a_pForces->push_back(m_forces[i]);
    }
}


/// returns torques at clipped panel centroids
void ShipForces::getTorques(std::vector<mml::Vector3d>* a_pTorques) const
{
    a_pTorques->clear();
    int n = m_torques.size();
    for (int i = 0; i < n; ++i)
    {
        a_pTorques->push_back(m_torques[i]);
    }
}


#endif // DEMO


